#include "cw_pid.h"

PIDDef LspdPID;
PIDDef RspdPID;

PIDDef LposPID;
PIDDef RposPID;

PIDDef TlinePID;

//PID参数初始化
void PID_Init(void)
{
	//左轮速度PID，较慢
	PIDSetParam(&LspdPID,2,0.1,10,0,0,0);
	//右轮速度PID，较慢
	PIDSetParam(&RspdPID,2,0.1,10,0,0,0);

	//PIDSetParam(&TlinePID,7,0,4,0,0,0);//0.5(HJtrack)
	PIDSetParam(&TlinePID,0.7,0.01,14,50,0,0);//

}

int Position_PID(int reality,int target,PIDDef* hPID)
{
	//计算本次误差
	hPID->Bias = target-reality;
	//PID输出运算
	hPID->Pid_out =(hPID->Kp * hPID->Bias)      						/* 比例环节 */
				  //+(hPID->Ki*hPID->Bias)												/* 积分环节 */
				  +(hPID->Kd * (hPID->Bias - hPID->Last_bias)); 	/* 微分环节 */;
	//更新上次误差(本次误差传递给上次误差）
	hPID->Last_bias=hPID->Bias;
	
	//返回整数结果
	return (int)hPID->Pid_out;
}
int Incremental_PID(int reality,int target,PIDDef* hPID)
{
	//计算本次误差
	hPID->Bias = target-reality;
	//PID输出运算
	hPID->Pid_out=hPID->Kp*(hPID->Bias-hPID->Last_bias)
				 +hPID->Ki*hPID->Bias
				 +hPID->Kd*((hPID->Bias-hPID->Last_bias)-(hPID->Last_bias-hPID->Prev_bias));
	//更新上上次误差(上次误差传递给上上次误差）
	hPID->Prev_bias=hPID->Last_bias;
	//更新上次误差(本次误差传递给上次误差）
	hPID->Last_bias=hPID->Bias;
	//返回整数结果
	return (int)hPID->Pid_out;
}

int Position_PIDpro(int reality,int target,PIDDef* hPID)
{
	//计算偏差
	hPID->Bias=target-reality;
	//偏差累计
	hPID->Integral_bias+=hPID->Bias;
	//积分限幅
    if(hPID->Integral_bias> hPID->IntegralMAX)
    	hPID->Integral_bias = hPID->IntegralMAX;
    if(hPID->Integral_bias< -hPID->IntegralMAX)
    	hPID->Integral_bias = -hPID->IntegralMAX;
		//PID运算
    hPID->Pid_out = (hPID->Kp*hPID->Bias)                 /* 比例环节 */
         +(hPID->Ki*hPID->Integral_bias)                  /* 积分环节 */
         +(hPID->Kd*(hPID->Bias-hPID->Last_bias));        /* 微分环节 */
    //偏差传递
    hPID->Last_bias=hPID->Bias;
    //返回整数结果
    return(int)hPID->Pid_out;
}

//PID参数设置函数
void PIDSetParam(PIDDef *hPID,float Kp,float Ki,float Kd,float Imax,int Deadzone,int Izone)
{
	hPID->Kp=Kp;
	hPID->Ki=Ki;
	hPID->Kd=Kd;
	hPID->IntegralMAX=Imax;
	hPID->Dead_zone=Deadzone;
	hPID->Integral_zone=Izone;
}